// Have include guard.
#ifndef STATEMACHINECONTROLLER_H
#define STATEMACHINECONTROLLER_H

// Include files.
#include <list>
#include <memory>
#include "RobotController.hpp"
#include "State.hpp"
#include "FinalState.hpp"

/**
 *  The StateMachineController class uses a state machine system to control the robot.
 *  
 *  To use it, create subclasses that override the Initialize.
 *  In Initialize, add the States and trasitions to those States.
 *  
 *  This class seems to work.
 */
class StateMachineController :
	public RobotController
{
	public:
		StateMachineController(bool deactivateOnFinish = true);
		virtual ~StateMachineController();
	protected:
		virtual void Initialize(); 
		virtual void ActivateImpl();
		virtual void Periodic();
		virtual void Continuous();
		State &GetInitialState();
		void SetInitialState(State &state);
		State &GetFinalState();
		void OwnState(State &state);
	private:
		bool m_deactivateOnFinish;
		State *m_stateInitial;
		State *m_stateCurrent;
		FinalState m_stateFinal;
		
		// TODO: Switch to shared_ptr.
		std::list<State*> m_statesOwned;
};

#endif // #ifndef STATEMACHINECONTROLLER_H 
